/**
 * @FilePath     : \simulations\include\agvWorld\tools.h
 * @Description  : 工具类定义
 * @Author       : xiujun.yang
 * @LastEditors  : error: error: git config user.name & please set dead value or install git && error: git config user.email & please set dead value or install git & please set dead value or install git
 * @LastEditTime : 2024-12-19 15:25:17
 * @Copyright (c) 2024 by lgim, All Rights Reserved. 
**/

#pragma once

#include "z_types.h"
#include <errno.h>
#include <functional>
#include "node_manager.hpp"

// #1 本地调试仿真宏定义
//  #define AGV_DEBUG_NAV

// #2 使用新的激光定位接口(定位topic pose名称与格式兼容问题)
//  #define USE_NEW_LASER_LOC_INTERFACE

// # 1-1 如果开启了本地仿真,开启无导航模式(关闭则正常导航模式)
#ifdef AGV_DEBUG_NAV
   #define AGV_DEBUG_NO_GUIDE
#endif

#define RCLCPP_ENV_OUT
// #define USE_DEBUG_HEAD

inline std::string GetFileName(const std::string& path) {
    size_t pos = path.find_last_of("/\\");
    std::string filename = (std::string::npos == pos) ? path : path.substr(pos + 1);
    size_t dotPos = filename.find_last_of('.');
    return (std::string::npos == dotPos) ? filename : filename.substr(0, dotPos);
}

#ifdef USE_DEBUG_HEAD
#define LOG_WITH_FUNC(info) "(" << GetFileName(__FILE__) << ":" <<  __LINE__  << ")" << info 
#else
#define LOG_WITH_FUNC(info) info 
#endif

#ifdef RCLCPP_ENV_OUT
#include "rclcpp/rclcpp.hpp"
#define DEBUG_OUT(info) RCLCPP_INFO_STREAM(rclcpp::get_logger("navigator"), LOG_WITH_FUNC(info))
#define DEBUG_OUT_THROTTLE(logger,clock,s,info) RCLCPP_INFO_STREAM_THROTTLE(logger, clock, s, LOG_WITH_FUNC(info)) 
#define DEBUG_WARN_OUT(info) RCLCPP_WARN_STREAM(rclcpp::get_logger("navigator"), info)
#define DEBUG_ERROR_OUT(info) RCLCPP_ERROR_STREAM(rclcpp::get_logger("navigator"), info)
#define DEBUG_FATAL_OUT(info) RCLCPP_FATAL_STREAM(rclcpp::get_logger("navigator"), info)
#else
#include <iostream>
#define DEBUG_OUT(info) std::cout << info << std::endl
#define DEBUG_OUT_THROTTLE(s,info) std::cout << info << std::endl
#define DEBUG_WARN_OUT(info) std::cout << info << std::endl
#define DEBUG_ERROR_OUT(info) std::cout << info << std::endl
#define DEBUG_FATAL_OUT(info) std::cout << info << std::endl
#endif

#define FLOAT_GATE 1e-5

#define BIT(x) (((USHORT)1) << (x))
#define BITL(x) (((UINT)1) << (x))
#define TestBit(x, i) ((x & BIT(i)) != 0)
#define SIGN(x) ((x >= 0) ? 1 : -1) // The sign of variable x

// Defines PI
constexpr double PI = 3.14159265;
constexpr double PI_2 = PI / 2.0;
constexpr double _2_PI = PI * 2.0;
constexpr double PI_4 = PI / 4.0;
#define TO_DEGREE(x) (x / PI * 180.0f)
#define TO_RADIAN(x) (x / 180.0f * PI)

//////////////////////////////////////////////////////////////////////////////

// Limit "x" within "[-LimitVal, LimitVal]" LimitVal > 0 ?
template <class Type>
Type Limit(Type x, Type LimitVal)
{
    if (x > LimitVal)
        return LimitVal;

    else if (x > -LimitVal)
        return x;

    else
        return -LimitVal;
}

// Limit "x" within "[LimitLowVal, LimitUpVal]"
template <class Type>
Type LimitBetween(Type x, Type LimitLowVal, Type LimitUpVal)
{
    if (x > LimitUpVal)
        x = LimitUpVal;

    if (x < LimitLowVal)
        x = LimitLowVal;

    return x;
}

// Caculate the square of a value
template <class Type>
Type Square(Type x)
{
    return (Type)(x * x);
}

//
// Let "from" approaches "to" at the increament of "step".
//
// Note:
//    1. "from" and "to" can be any value, but "fStep" should be positive.
//    2. If "from" reaches "to", TRUE is returned.
//
template <class Type>
BOOL Approach(Type &from, Type to, Type step)
{
    if (from + step < to)
    {
        from += step;
        return FALSE;
    }

    else if (from - step > to)
    {
        from -= step;
        return FALSE;
    }

    else
    {
        from = to;
        return TRUE;
    }
}
// #endif  //LINUX_PLATFORM_USING

// Estimate covering area
inline float EstimateArea(float fStartValue, float fEndValue, float fSlope)
{
    // If the start value is equal to the end value, return 0
    if (fStartValue == fEndValue)
        return 0.0f;

    // Estimate the covering area
    return (float)(fabs((Square(fEndValue) - Square(fStartValue)) / (2 * fSlope)));
}

// Check if 2 floats are approximately equal
inline BOOL ApprEqual(float x, float y, float fGate = FLOAT_GATE)
{
    return (fabs(x - y) < fGate);
}

inline float FabsAngleDiff(float angle1, float angle2)
{
    if (angle1 > 10000.0 || angle1 < -10000.0)
    {
        angle1 = PI / 3;
    }

    if (angle2 > 10000.0 || angle2 < -10000.0)
    {
        angle2 = PI / 3;
    }

    while (angle1 < 0)
        angle1 += 2 * PI;
    while (angle1 > 2 * PI)
        angle1 -= 2 * PI;
    while (angle2 < 0)
        angle2 += 2 * PI;
    while (angle2 > 2 * PI)
        angle2 -= 2 * PI;
    float diff = (float)fabs(angle1 - angle2);
    if (diff <= PI)
        return diff;
    else
        return 2 * PI - diff;
}

typedef struct _stTimer
{
    int iHour;
    int iMin;
    int iSec;

} stTime;

typedef unsigned long long TIME_TYPE;
// replace windows GetTickCount
inline TIME_TYPE GetTickCount()
{
    //    struct timespec ts;  //change by QF
    //    clock_gettime(CLOCK_MONOTONIC, &ts);
    //    return (ts.tv_sec * 1000 + ts.tv_nsec / 1000000);

    struct timespec ts;
    clock_gettime(CLOCK_MONOTONIC, &ts);
    return ((unsigned long long)(ts.tv_nsec / 1000000) + (unsigned long long)(ts.tv_sec * 1000ull));
}

inline void Sleep(int ms)
{
    usleep(ms * 1000);
}

inline void TurnMsTimeToNormTime(const long long msec, stTime &stOutTime)
{
    long long sec = msec / 1000;
    long long min = sec / 60;
    sec -= min * 60;
    long long hour = min / 60;
    min -= hour * 60;

    stOutTime.iHour = hour;
    stOutTime.iMin = min;
    stOutTime.iSec = sec;
}

// 1.SCHED_OTHER Time sharing scheduling strategy:
// 2. SCHED_FIFO Real time scheduling strategy, first come first service. Once the CPU is occupied, it will run all the time. Run until a higher priority task arrives or abandons itself
// 3. SCHED_RR   Real time scheduling strategy, time slice rotation. When the process runs out of time slices, the system will reallocate the time slices and put them at the end of the ready queue.
//               Put at the end of the queue ensures the fairness of scheduling all RR tasks with the same priority
// 4. SCHED_OTHER Priority is not supported, but SCHED_FIFO and SCHED_RR is support the use of priority, they are 1 and 99, the higher the value, the higher the priority.

inline void SetPthreadPriority(const int iSchedulingAlgorithms, const int iPthreadPolicy, pthread_attr_t &PthreadAttr)
{
    struct sched_param param;

    if (pthread_attr_init(&PthreadAttr) != 0)
    {
        DEBUG_OUT("pthread_attr_init failed");
    }

    if (pthread_attr_setschedpolicy(&PthreadAttr, iSchedulingAlgorithms) != 0)
    {
        DEBUG_OUT("pthread_attr_setschedpolicy failed");
    }

    param.sched_priority = iPthreadPolicy;

    if (pthread_attr_setschedparam(&PthreadAttr, &param) != 0)
    {
        DEBUG_OUT("pthread_attr_setschedparam failed");
    }
}

// 默认bIsAutoRecoveryResources = true 情况下 不需要pthread_join进行线程资源回收
inline int BeginThread(void *(*__start_routine)(void *),
                       void *__restrict __arg,
                       pthread_t *__restrict __newthread,
                       const int iPthreadPolicy = THREAD_PRIORITY_NORMAL,
                       const int iSchedulingAlgorithms = SCHED_RR,
                       const bool bIsAutoRecoveryResources = true)
{
    // On success, pthread_create() returns 0; on error,
    // it returns an error number, and the contents of *thread are undefined.
    struct sched_param param;
    pthread_attr_t PthreadAttr;
    if (pthread_attr_init(&PthreadAttr) != 0)
    {
        DEBUG_OUT("pthread_attr_init failed");
    }
    if (bIsAutoRecoveryResources)
        if (pthread_attr_setdetachstate(&PthreadAttr, PTHREAD_CREATE_DETACHED) != 0) // 设置线程属性 由系统释放线程资源
        {
            DEBUG_OUT("pthread_attr_setdetachstate failed");
        }
    if (pthread_attr_setschedpolicy(&PthreadAttr, iSchedulingAlgorithms) != 0)
    {
        DEBUG_OUT("pthread_attr_setschedpolicy failed");
    }
    param.sched_priority = iPthreadPolicy;
    if (pthread_attr_setschedparam(&PthreadAttr, &param) != 0)
    {
        DEBUG_OUT("pthread_attr_setschedparam failed");
    }
    int iRet = -1;
    iRet = pthread_create(__newthread, &PthreadAttr, __start_routine, __arg);
    if (iRet == EAGAIN)
    {
        DEBUG_OUT("pthread_create failed , Insufficient resources to create another thread");
    }
    else if (iRet == EINVAL)
    {
        DEBUG_OUT("pthread_create failed , Invalid settings in attr");
    }
    else if (iRet == EPERM)
    {
        DEBUG_OUT("pthread_create failed , No permission to set the scheduling policy and parameters specified in attr");
    }
    else if (iRet == ENOMEM)
    {
        DEBUG_OUT("pthread_create failed , Out of memory");
    }
    pthread_attr_destroy(&PthreadAttr);
    return iRet;
}

inline char *_tcsncpy(char *dest, const char *src, size_t n)
{
    return strncpy(dest, src, n);
}
inline char *_tcsncpy(char *dest, string src, size_t n)
{
    return strncpy(dest, src.c_str(), n);
}
inline char *_tcscpy(char *dest, const char *src)
{
    return strcpy(dest, src);
}
inline char *_tcscpy(char *dest, string src)
{
    return strcpy(dest, src.c_str());
}

inline void SecureZeroMemory(void *in, unsigned long size)
{
    try
    {
        memset(in, 0, size);
    }
    catch (const std::exception &e)
    {
        std::cerr << e.what() << '\n';
        DEBUG_FATAL_OUT("crash:memset SecureZeroMemory" << std::string(e.what()));
    }
    catch (...)
    {
        DEBUG_FATAL_OUT("crash:memset SecureZeroMemory(other reason)");
    }
}

inline struct tm *GetCurrentTime()
{
    time_t lTime = 0;
    time_t tRetTime;
    tRetTime = time(&lTime);
    struct tm *lpstLocalTime = NULL;
    if (tRetTime)
    {
        lpstLocalTime = localtime(&lTime);
    }
    return lpstLocalTime;
}

inline int WaitforThreadSignal(sem_t *pInPutSem, int iMs)
{
    if (pInPutSem != NULL)
    {
        int iSec = iMs / 1000;
        int iNSec = (iMs % 1000) * 1000 * 1000;

        struct timespec ts;

        if (clock_gettime(CLOCK_REALTIME, &ts) < 0)
            return -1;

        ts.tv_sec += iSec;
        ts.tv_nsec += iNSec;

        ts.tv_sec += ts.tv_nsec / 1000000000; // Nanoseconds [0 .. 999999999]
        ts.tv_nsec = ts.tv_nsec % 1000000000;

        return sem_timedwait(pInPutSem, &ts); // block
        // return 0 on success; on error, the value of the semaphore is left unchanged, -1 is returned, and errno is set to indicate the error
    }
    return -1;
}

inline sem_t *CreateSem(BOOL __pshared, BOOL __value)
{
    sem_t *sem_tTemp = new sem_t;
    sem_init(sem_tTemp, __pshared, __value);
    return sem_tTemp;
}

inline void DestroySem(sem_t *__sem)
{
    if (__sem)
    {
        sem_destroy(__sem);
    }
}

inline void AddSem(sem_t *__sem)
{
    if (__sem)
    {
        sem_post(__sem);
    }
}

inline int PthreadExit()
{
    pthread_exit(NULL);
}
inline int PthreadJoin(pthread_t ThreadNum)
{
    void *thrd_ret;
    return pthread_join(ThreadNum, &thrd_ret);
}
// #endif
// std::stoi stol stoul stoll stof stod
inline bool string2Number(const std::string &str, long long &ret)
{
    bool ok = true;
    try
    {
        ret = stoll(str);
    }
    catch (...)
    {
        ok = false;
        DEBUG_OUT("error: string2longlong failed");
    }
    return ok;
}

inline bool string2Number(const std::string &str, int &ret)
{
    bool ok = true;
    try
    {
        ret = stoi(str);
    }
    catch (...)
    {
        ok = false;
        DEBUG_OUT("error: string2Int failed");
    }
    return ok;
}

inline bool string2Double(const std::string &str, double &ret)
{
    bool ok = true;
    try
    {
        ret = stod(str);
    }
    catch (...)
    {
        // everything else
        ok = false;
        DEBUG_OUT("error: string2Double failed");
    }
    return ok;
}

inline bool string2Float(const std::string &str, float &ret)
{
    bool ok = true;
    try
    {
        ret = stof(str);
    }
    catch (...)
    {
        // everything else
        ok = false;
        DEBUG_OUT("error: string2Float failed");
    }
    return ok;
}
inline double getRemainder(double x, double y)
{
    int n = trunc(x / y);
    return x - n * y;
}

inline double normalizationRad(double rad)
{
    double tmp = getRemainder(rad, 2 * M_PI);
    if (tmp > M_PI)
        return tmp - 2 * M_PI;
    if (tmp < -1 * M_PI)
        return tmp + 2 * M_PI;

    return tmp;
}

inline double getRadDistance(double target, double current)
{
    return normalizationRad(current - target);
}

